package com.googlecode.curvedmetromaps.core.logic.forcedriven;

import org.jbox2d.common.Vec2;
import org.jbox2d.dynamics.Body;

import com.googlecode.curvedmetromaps.core.logic.costfunction.CostConstants;

/**
 * Constraint S3: forces that push vertices apart.
 */
public class ForceS3 extends ForceSubsystem {
	
	ForceS3(ForceSystem system) {
		super(system);
	}
	
	void applyForces(int delta) {
		for (int m1 = 0; m1 < system.stationBodies.length; m1++) {
			for (int m2 = m1 + 1; m2 < system.stationBodies.length; m2++) {
				Body m1body = system.stationBodies[m1];
				Body m2body = system.stationBodies[m2];
				Vec2 p1 = m1body.getPosition();
				Vec2 p2 = m2body.getPosition();

				Vec2 direction = p1.sub(p2);
				float len = direction.normalize();
				
				if (len < CostConstants.S3_DVMIN) {
					double f = ForceConstants.S3_REPULSE * Math.exp(-CostConstants.S3_LAMBDA * len / CostConstants.S3_DVMIN);
	
					direction.mulLocal((float) f);
	
					m1body.applyForceToCenter(direction);
					m2body.applyForceToCenter(direction.negateLocal());
				}
			}
		}
	}
}
